#include "AnaDef.h"
TLinearFitter fitter(1);
  
const Double_t EPSILON = 1e-12;

//===========
Double_t linearFit(const Int_t np, const Double_t xarr[], const Double_t yarr[], const Double_t eys[], const Double_t xtarget, const Int_t order)
{
  //chi2 polinomial fit of degree, i.e. up to x^(deg-1), see also Recipes p673.
  const Int_t deg = order+1;
  TMatrixD leftmatrix(deg, deg);
  TMatrixD rightvector(deg,1);

  //loop over data point
  for(Int_t ip=0; ip<np; ip++){
    const Double_t xx = xarr[ip]-xtarget;
    const Double_t yy = yarr[ip];
    //to avoid ey<0 or ey=0
    const Double_t ey = TMath::Max(EPSILON, eys[ip]);

    //starting from [0][0] term
    Double_t lastrecord = 1/ey/ey;    
    //loop over each matrix entry once with increasing power of xx
    for(Int_t sumij=0; sumij<=(deg-1)*2; sumij++){
      for(Int_t ii=TMath::Max( 0, sumij-(deg-1) ); ii<=TMath::Min(deg-1, sumij); ii++){
        const Int_t jj = sumij - ii;
        //printf("%d: %d %d\n", sumij, ii, jj);
        
        leftmatrix[ii][jj] += lastrecord;
      }
      //printf("sum %d\n\n",sumij);
      lastrecord *= xx;
    }

    //loop over vector entry
    lastrecord = yy/ey/ey;
    for(Int_t ii=0; ii<deg; ii++){
      rightvector[ii][0] +=  lastrecord;
      lastrecord *= xx;
    }

    //checked by hand, correct!
    //leftmatrix.Print();
    //printf("===\n");
    //rightvector.Print();
  }

  /*
TMatrixD a(2,2)
a[0][0]=cos(3)
a[0][1]=sin(3)
a[1][0]=-sin(3)
a[1][1]=cos(3)
a.Print()
TDecompLU lu(a)
TMatrixD c=lu.Invert()
c.Print()
a.Print()
  */

  //leftmatrix
  TDecompSVD mdec(leftmatrix);
  if(!mdec.Decompose()){
    printf("MN2D::LinearFit matrix singular?\n");
    for(Int_t ii=0; ii<np; ii++){
      printf("%d: %e %e %e - %e\n", ii, xarr[ii], yarr[ii], eys[ii], xtarget);
    }
    leftmatrix.Print();
    exit(1);
  }
  const TMatrixD inv=mdec.Invert();

  Double_t par0 = 0;
  for(Int_t ii=0; ii<deg; ii++){
    par0 += inv[0][ii]*rightvector[ii][0];
  }

  //compared to LinearFitter::Eval, consistent
  return par0;
}

Double_t GetSmoothEvalY(const Int_t npo, const Double_t xarr[], const Double_t yarr[], const Double_t eys[], const Double_t xtarget, const Char_t * formu)
{
  fitter.SetFormula(formu);
  fitter.ClearPoints();
  for(Int_t ii=0; ii<npo; ii++){
    Double_t xi = xarr[ii]-xtarget;
    fitter.AddPoint(&xi, yarr[ii], eys[ii]);
  }
  //fitter.EvalRobust();
  fitter.Eval();
  return fitter.GetParameter(0);
}

int main(){
  const Double_t xtarget = 1.5;
  const Double_t xx[]={0,1,2,9, 90, 100};
  const Double_t yy[]={3,60,10,50, 90, 100};
  const Double_t ey[]={0.03, 0.06, 0.1, 0.5, 0.9, 1};

  /*
  const Double_t xtarget = 9.750000e-01;
  const Double_t xx[]={8.750000e-01, 9.250000e-01, 1.025000e+00, 1.075000e+00};
  const Double_t yy[]={3.426058e-03, 5.910281e-03, 4.796787e-06, 0.000000e+00};
  const Double_t ey[]={1.302977e-03, 1.711368e-03, 4.875449e-05, 0.000000e+00};
  */

  const Int_t np=sizeof(xx)/sizeof(Double_t);

  const Double_t ev0 = linearFit(np, xx, yy, ey, xtarget, 3); 
  printf("ev0  %e\n", ev0);

  /*
  const Int_t npar = fitter->GetNumberTotalParameters();
  for(Int_t ii=0; ii<npar; ii++){
    printf("fitter %d: %e\n", ii, fitter->GetParameter(ii));
  }
  */

  //jjj 7.896699e+00
  const Double_t yval = GetSmoothEvalY(6, xx, yy, ey, xtarget,"1++x++x*x++x*x*x"); 
  //const Double_t yval = GetSmoothEvalY(np, xx, yy, ey, xtarget,"1++x++x*x"); 
  printf("ttt  %e\n", yval);

  //const Double_t yval2 = GetSmoothEvalY(6, xx, yy, ey, xtarget); 
  //printf("ttt2 %e\n", yval2);

  /*
  TGraphErrors * gr= new TGraphErrors(4, xx, yy, 0x0, ey);
  gr->Fit("pol2");
  for(Int_t ii=0; ii<npar; ii++){
    const Double_t pp=gr->GetFunction("pol2")->GetParameter(ii);
    printf("gr %d: %e -- %e\n", ii, pp, fitter->GetParameter(ii)-pp);
  }
  */

  return 0;
}
